#include "motor_driver.h"
#include "imu_driver.h"
#include "pid.h"
#include"spi_wifi.h"
#include"camera.h"
#include"leg_control.h"
#include"pid_balence.h"
#include"jump_control.h"
#include"open_count.h"
#include"cpu0init.h"
uint8 jump_fangxiang = 1;
float avarage_speed = 0;
int set_speed = 0;
struct PID_parameters pid_loc_v_angle =
{
  .kp =  9,//30,
  .ki = 0.41,//0,
  .kd = 2,//10,
  .max_kp_product = 10000,
  .max_ki_product = 10000,
  .max_kd_product = 10000,
  .max_pid_sum = 10000,
};
struct PID_parameters pid_loc_a_angle =
{
  .kp = 8, //16,
  .ki = 0,//2,
  .kd =2,//8,
  .max_kp_product = 700,
  .max_ki_product = 700,
  .max_kd_product = 700,
  .max_pid_sum = 700,
};
struct PID_parameters pid_inc_v_avarage =
{
  .kp = 0.05,
  .ki = 0.0005,
  .kd = 0.00,
  .max_kp_product = 22,
  .max_ki_product = 22,
  .max_kd_product = 22,
  .max_pid_sum = 22,
};

struct PID_parameters pid_loc_yaw_angle =
{
  .kp = 15,//21,
  .ki = 0.1,//2,
  .kd = 5,//7,
  .max_kp_product = 10000,
  .max_ki_product = 10000,
  .max_kd_product = 10000,
  .max_pid_sum = 10000,
};



struct PID_parameters pid_inc_roll_angle =
{
  .kp = 0.001,//21,
  .ki = 0.01,//2,
  .kd = 0.06,//7,
  .max_kp_product = 13,
  .max_ki_product = 13,
  .max_kd_product = 13,
  .max_pid_sum = 13,
};

struct pid_balence_s pid_balence;


void plane_balence_torque_calculate(void)
{

    local_pid(&pid_loc_a_angle,0,imu_0.e_angle.pitch);
    local_pid(&pid_loc_v_angle,pid_loc_a_angle.pid_sum,imu_0.gy/PI*180);
    pid_balence.plane_balence_torque = pid_loc_v_angle.pid_sum ;
    if (pid_balence.mode.plane == plane_jump)
    {
      pid_balence.plane_balence_torque *= 0.34;
    }
    



}

void steering_torque_calculate(void)
{



    if (pid_balence.mode.steering == steering_danbianqiao)
    {
        local_pid(&pid_loc_yaw_angle,-0,imu_0.gz/PI*180);
        pid_balence.steering_torque = pid_loc_yaw_angle.pid_sum;
        //pid_balence.steering_torque *= 0.5;
    }
    else if (pid_balence.mode.steering == steering_guizhong)
    {
        local_pid(&pid_loc_yaw_angle,-turning_radius_m*1.3,imu_0.gz/PI*180);
        pid_balence.steering_torque = pid_loc_yaw_angle.pid_sum;
        //pid_balence.steering_torque *= 0.7;
    }
    else if (pid_balence.mode.steering == steering_chuhuandao)
    {
        local_pid(&pid_loc_yaw_angle,open_count.open_v_yaw/PI*180,imu_0.gz/PI*180);
        pid_balence.steering_torque = pid_loc_yaw_angle.pid_sum;
        //pid_balence.steering_torque *= 0.7;
    }
    else if (pid_balence.mode.steering == steering_barrier_first)
    {
        if(jump_fangxiang == 0)
        {
        local_pid(&pid_loc_yaw_angle,-0.4*motor.average_v,imu_0.gz/PI*180);
        pid_balence.steering_torque = pid_loc_yaw_angle.pid_sum;
        }
        else if (jump_fangxiang == 1)
        {
            local_pid(&pid_loc_yaw_angle,0.4*motor.average_v,imu_0.gz/PI*180);
            pid_balence.steering_torque = pid_loc_yaw_angle.pid_sum;
        }
        //pid_balence.steering_torque *= 0.7;
    }
    else if (pid_balence.mode.steering == steering_barrier_second)
    {
        if(jump_fangxiang == 0)
        {
        local_pid(&pid_loc_yaw_angle,0.3*motor.average_v,imu_0.gz/PI*180);
        pid_balence.steering_torque = pid_loc_yaw_angle.pid_sum;
        }
        else if (jump_fangxiang == 1)
        {
            local_pid(&pid_loc_yaw_angle,-0.3*motor.average_v,imu_0.gz/PI*180);
            pid_balence.steering_torque = pid_loc_yaw_angle.pid_sum;
        }
        //pid_balence.steering_torque *= 0.7;
    }
    else if (pid_balence.mode.steering == steering_barrier_third)
    {
        local_pid(&pid_loc_yaw_angle,0,imu_0.gz/PI*180);
        pid_balence.steering_torque = pid_loc_yaw_angle.pid_sum;
        //pid_balence.steering_torque *= 0.7;
    }
    else if (pid_balence.mode.steering == steering_stop)
    {
        local_pid(&pid_loc_yaw_angle,-0,imu_0.gz/PI*180);
        pid_balence.steering_torque = pid_loc_yaw_angle.pid_sum;
        //pid_balence.steering_torque *= 0.7;
    }
    else if (pid_balence.mode.steering == steering_jump)
    {

        pid_loc_yaw_angle.i_error =0;
        pid_balence.steering_torque = 0;
        //pid_balence.steering_torque *= 0.7;
    }
    else
    {

        local_pid(&pid_loc_yaw_angle,-turning_radius_m,imu_0.gz/PI*180);
        pid_balence.steering_torque = pid_loc_yaw_angle.pid_sum;
    }


}


void leg_place_calculate(void)
{

  
    //inc_pid(&pid_inc_roll_angle,0,imu_0.e_angle.roll);
    // if(pid_inc_roll_angle.pid_sum > 0)
    // chassis_set_position(&chassis,INIT_LENGTH+pid_inc_roll_angle.pid_sum,-pid_inc_v_avarage.pid_sum,INIT_LENGTH,-pid_inc_v_avarage.pid_sum);

    // if(pid_inc_roll_angle.pid_sum <=  0)
    // chassis_set_position(&chassis,INIT_LENGTH,-pid_inc_v_avarage.pid_sum,INIT_LENGTH-pid_inc_roll_angle.pid_sum,-pid_inc_v_avarage.pid_sum);


    //     float dt_speed;
    // static float last_dt=0;
    // dt_speed = imu_0.gz/2.0f*0.8f*0.0005f+0.9995f*last_dt;
    // last_dt = dt_speed;


    //    if(dt_speed > 0)
    //    {
    //     y += dt_speed;
    //     y = MINMAX(y,INIT_LENGTH,5.5);
    //     float new_l = sqrt(x*x+y*y);
    //     float new_angle =   atan2f(x,y)/PI*180;
    //     new_angle = MINMAX(new_angle,-27,27);
    //    chassis_set_position(&chassis,new_l,-new_angle,INIT_LENGTH,-pid_inc_v_avarage.pid_sum);
    //    }
    //    else
    //    {
    //     y -= dt_speed;
    //     y = MINMAX(y,INIT_LENGTH,5.5);
    //     float new_l = sqrt(x*x+y*y);
    //     float new_angle =    atan2f(x,y)/PI*180;
    //     new_angle = MINMAX(new_angle,-27,27);
    //    chassis_set_position(&chassis,INIT_LENGTH,-pid_inc_v_avarage.pid_sum,new_l,-new_angle);
    //    }
    brushless_motor_get_speed(&motor);
    if (pid_balence.mode.leg == leg_normal)
    {

        inc_pid(&pid_inc_v_avarage,avarage_speed,motor.average_v);


      pid_balence.left_leg.angle = -pid_inc_v_avarage.pid_sum;
      pid_balence.left_leg.length = INIT_LENGTH;
      pid_balence.right_leg.angle = -pid_inc_v_avarage.pid_sum;
      pid_balence.right_leg.length = INIT_LENGTH;
    }
    else if (pid_balence.mode.leg == leg_danbianqiao)
    {

        if(abs(motor.speed_left-danbianqiaosudu) > abs(motor.speed_right-danbianqiaosudu))
        {
            inc_pid(&pid_inc_v_avarage,danbianqiaosudu,motor.speed_right);
        }
        else
        {
            inc_pid(&pid_inc_v_avarage,danbianqiaosudu,motor.speed_left);
        }
        float x ;
        x = INIT_LENGTH_BIRDGE * sinf(pid_inc_v_avarage.pid_sum/180*PI);
        float y;
        y = INIT_LENGTH_BIRDGE * cosf(pid_inc_v_avarage.pid_sum/180*PI);
      //inc_pid(&pid_inc_v_avarage,avarage_speed,motor.average_v);
        inc_pid(&pid_inc_roll_angle,0,imu_0.e_angle.roll);


      if(pid_inc_roll_angle.pid_sum > 0)
       {
        y += pid_inc_roll_angle.pid_sum;
        y = MINMAX(y,INIT_LENGTH_BIRDGE,13);
        float new_l = sqrt(x*x+y*y);
        float new_angle =   atan2f(x,y)/PI*180;
        new_angle = MINMAX(new_angle,-27,27);
        pid_balence.left_leg.angle = -new_angle;
        pid_balence.left_leg.length = new_l;
        pid_balence.right_leg.angle = -pid_inc_v_avarage.pid_sum;
        pid_balence.right_leg.length = INIT_LENGTH_BIRDGE;
       }
       else
       {
        y -= pid_inc_roll_angle.pid_sum;
        y = MINMAX(y,INIT_LENGTH_BIRDGE,13);
        float new_l = sqrt(x*x+y*y);
        float new_angle =    atan2f(x,y)/PI*180;
        new_angle = MINMAX(new_angle,-27,27);
        pid_balence.left_leg.angle = -pid_inc_v_avarage.pid_sum;
        pid_balence.left_leg.length = INIT_LENGTH_BIRDGE;
        pid_balence.right_leg.angle = -new_angle;
        pid_balence.right_leg.length = new_l;
       }
    }
    else if (pid_balence.mode.leg == leg_jump)
    {
      jump_control();
    }
        else if (pid_balence.mode.leg == leg_yawan)
    {
      /* code */
    }

    



}





void dynamic_pid_balence(void)


{
    static last_error = 0;
    float temp = turning_radius_m;
    turning_radius_m = turning_radius_m*0.2 + 0.04 *last_error;
    last_error = temp;
    if (stop_flag == 2)
    {
        avarage_speed = 0;
    }
    else
    {
        avarage_speed = set_speed;
    }
    plane_balence_torque_calculate();
    steering_torque_calculate();
    leg_place_calculate();
    chassis_set_position(&chassis,pid_balence.left_leg.length,pid_balence.left_leg.angle,pid_balence.right_leg.length,pid_balence.right_leg.angle);
    //brushless_motor_set_torque(&motor,2000 ,2000);
    brushless_motor_set_torque(&motor,MINMAX((pid_balence.plane_balence_torque + pid_balence.steering_torque),-10000,10000),MINMAX((pid_balence.plane_balence_torque - pid_balence.steering_torque),-10000,10000));
}


void dynamic_pid_balence_init(void)
{
  pid_balence.mode.plane = plane_normal;
  pid_balence.mode.steering = steering_normal;
  pid_balence.mode.leg = leg_normal;
  
}

// void pid_balence_normal(void)
// {




//     turning_radius_m = turning_radius_m*0.13;
//     float dt_speed;
//     brushless_motor_get_speed(&motor);
//     now_speed = (motor.speed_left +motor.speed_right)/2;
//     //dt_speed = motor.speed_left - motor.speed_right;
//     static float last_dt=0;
//     dt_speed = imu_0.gz/2.0f*0.8f*0.0005f+0.9995f*last_dt;
//     last_dt = dt_speed;
//     inc_pid(&pid_inc_v_avarage,avarage_speed,now_speed);
//     float x ;
//     x = INIT_LENGTH * sinf(pid_inc_v_avarage.pid_sum/180*PI);
//     float y;
//     y = INIT_LENGTH * cosf(pid_inc_v_avarage.pid_sum/180*PI);




//     //inc_pid(&pid_inc_roll_angle,0,imu_0.e_angle.roll);
//     if(pid_inc_roll_angle.pid_sum > 0)
//     chassis_set_position(&chassis,INIT_LENGTH+pid_inc_roll_angle.pid_sum,-pid_inc_v_avarage.pid_sum,INIT_LENGTH,-pid_inc_v_avarage.pid_sum);

//     if(pid_inc_roll_angle.pid_sum <=  0)
//     chassis_set_position(&chassis,INIT_LENGTH,-pid_inc_v_avarage.pid_sum,INIT_LENGTH-pid_inc_roll_angle.pid_sum,-pid_inc_v_avarage.pid_sum);

// //    if(dt_speed > 0)
// //    {
// //     y += dt_speed;
// //     y = MINMAX(y,INIT_LENGTH,5.5);
// //     float new_l = sqrt(x*x+y*y);
// //     float new_angle =   atan2f(x,y)/PI*180;
// //     new_angle = MINMAX(new_angle,-27,27);
// //    chassis_set_position(&chassis,new_l,-new_angle,INIT_LENGTH,-pid_inc_v_avarage.pid_sum);
// //    }
// //    else
// //    {
// //     y -= dt_speed;
// //     y = MINMAX(y,INIT_LENGTH,5.5);
// //     float new_l = sqrt(x*x+y*y);
// //     float new_angle =    atan2f(x,y)/PI*180;
// //     new_angle = MINMAX(new_angle,-27,27);
// //    chassis_set_position(&chassis,INIT_LENGTH,-pid_inc_v_avarage.pid_sum,new_l,-new_angle);
// //    }


//     local_pid(&pid_loc_a_angle,0,imu_0.e_angle.pitch);
//     local_pid(&pid_loc_v_angle,pid_loc_a_angle.pid_sum,imu_0.gy/PI*180);
//     local_pid(&pid_loc_yaw_angle,-turning_radius_m,imu_0.gz/PI*180);
//     //brushless_motor_set_torque(&motor,2000 ,2000);
//     brushless_motor_set_torque(&motor,MINMAX((pid_loc_v_angle.pid_sum + pid_loc_yaw_angle.pid_sum),-10000,10000),MINMAX((pid_loc_v_angle.pid_sum - pid_loc_yaw_angle.pid_sum),-10000,10000));
//     data[0] = imu_0.e_angle.pitch;
//     //data[1] = imu_0.e_angle.roll;
//     //data[2] = imu_0.e_angle.yaw;
//     data[3] = pid_loc_a_angle.pid_sum;;
//     data[4] = imu_0.gy/PI*180;
//     data[5] = pid_inc_v_avarage.pid_sum;
//     data[6] = -imu_0.e_angle.pitch;
//     data[7] = (motor.speed_left +motor.speed_right)/2;
//     data[8] = -turning_radius_m;
//     data[9] = imu_0.gz/PI*180;
// //
// //    //data[12] = 0;
// //    //data[13] = 0;
// //    data[14] = 0;
    
// }
